package it.gius.pe.algorithm.gjk;

import org.jbox2d.common.Vec2;

import it.gius.pe.SystemCostants;
import it.gius.pe.Shape.witness.Witness;
import it.gius.pe.algorithm.gjk.Simplex.SimplexEdge;

public class EpaAlgorithm {

	private static GjkUtilsFunctions utils = new GjkUtilsFunctions();

	public static void penetrationNormalAndDepth(IGjkAlgorithmShape shape, Simplex simplex, Vec2 q, SimplexEdge edgeSolution)
	{
		Simplex unboundSimplex = new Simplex(Simplex.UNBOUNDED_SIMPLEX);

		//SimplexEdge oldEdge = new SimplexEdge();
		
		unboundSimplex.set(simplex);
		
		unboundSimplex.computeWinding();

		Vec2 supportPoint = new Vec2();
		Vec2 pq = null;

		int dim = shape.getDim();

		int max = (dim*dim)/4;
		
		for(int i=0; i<max; i++)
		{
			utils.nearestEdgeInsidePointSimplex2D(unboundSimplex, q, edgeSolution);

			Witness witness = shape.supportPoint(edgeSolution.normal, supportPoint);

			pq = supportPoint.sub(q);

			float d = Vec2.dot(pq, edgeSolution.normal);

			if(d - edgeSolution.distance < SystemCostants.SQRT_EPSILON/*0.1*/)
			{
				/*if(d-edgeSolution.distance != 0)
					System.out.println("d-edgeSolution.distance: " + (d-edgeSolution.distance));*/
				
				edgeSolution.iterations = i;
				return;
			}
			else
			{
				/*if(edgeSolution.equals(oldEdge))
				{
					System.out.println("Epa not terminated but oldSolution equal to the new one");
					throw new NotConvergingException();
				}*/
				
				unboundSimplex.insert(supportPoint, witness, edgeSolution.indexB);
			}
			
			/*oldEdge.indexB = edgeSolution.indexB;
			oldEdge.distance = edgeSolution.distance;
			oldEdge.normal.set(edgeSolution.normal);*/

		}
		
		throw new NotConvergingException();


	}
}
